using System;
using System.Collections.Generic;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;
using UnityEngine;

namespace SocketWeaver.FixedMath
{
    public static class TransformHelper
    {
        //https://forum.unity.com/threads/quickly-retrieving-the-immediate-children-of-a-gameobject.39451/
        public static List<T> GetComponentsInDirectChildren<T>(Transform parent, bool includeInactive) where T : Component
        {
            List<T> tmpList = new List<T>();
            foreach (Transform transform in parent)
            {
                if (includeInactive || transform.gameObject.activeInHierarchy)
                {
                    tmpList.AddRange(transform.GetComponents<T>());
                }
            }
            return tmpList;
        }
    }

    public struct FTransformCache<T>
    {
        public bool valid;
        public T value;
    }

    /// <summary>
    /// small scale and negative scale not supported
    /// GameObject must be active to receive transform changes
    /// </summary>
    [ExecuteInEditMode]
    public class FTransform : MonoBehaviour
    {
        public bool interpolation = false;

        [SerializeField]
        FVector3 _localPosition = FVector3.zero;

        [SerializeField]
        FQuaternion _localRotation = FQuaternion.identity;

        [SerializeField]
        FVector3 _localScale = FVector3.one;

        [SerializeField]
        public bool editor_showDebugInfo = false;

        //parent and children
        //must call Refresh to update
        [SerializeField]
        FTransform _parent;
        [SerializeField]
        List<FTransform> _children = new List<FTransform>();


        //cached values, invalidated if localPosition, localRotation, and localScale changed
        //position
        FTransformCache<FVector3> _worldPositionCache;

        //rotation
        FTransformCache<FQuaternion> _worldQuaternionCache;
        FTransformCache<FVector3> _worldEulerAnglesCache;
        FTransformCache<FVector3> _localEulerAnglesCache;

        //matrix
        FTransformCache<FMatrix4x4> _localToWorldMatrixCache;
        FTransformCache<FMatrix4x4> _worldToLocalMatrixCache;

        //1: invalidate cache
        //2: notify parent
        //3: notify children 
        //4: find new parent
        //5: find new children
        public void ParentChanged()
        {
            InvalidateCache();

            if(_parent != null)
            {
                _parent.FindChildren();
            }
            FindParent();

            if(_children != null)
            {
                for(int i = 0; i < _children.Count; i++)
                {
                    FTransform child = _children[i];
                    child.InvalidateCache();
                    child.FindParent();
                }
            }

            FindChildren();
        }

        internal void InvalidateCache()
        {
            _worldPositionCache.valid = false;
        }

        internal void FindChildren()
        {
            _children = TransformHelper.GetComponentsInDirectChildren<FTransform>(transform, false);
        }

        internal void FindParent()
        {
            Transform parentTransform = transform.parent;
            _parent = parentTransform.GetComponent<FTransform>();
        }

        //properties
        public FTransform parent
        {
            get{
                return _parent;
            }
        }

        public FVector3 eulerAngles
        {
            get{
                if(_worldEulerAnglesCache.valid)
                {
                    return _worldEulerAnglesCache.value;
                }
                else
                {
                    _worldEulerAnglesCache.valid = true;

                    if(parent == null)
                    {   
                        _worldEulerAnglesCache.value = localEulerAngles;
                    }
                    else
                    {
                        _worldEulerAnglesCache.value = rotation.eulerAngles;
                    }

                    return _worldEulerAnglesCache.value;
                }
            }
            set{
                if(_worldEulerAnglesCache.valid)
                {
                    if(_worldEulerAnglesCache.value == value)
                    {
                        return;
                    }
                }

                if(parent == null)
                {
                    localEulerAngles = value;
                }
                else
                {
                    rotation = FQuaternion.FromEulerRad1(value * FMath.Deg2Rad);
                }

                _worldEulerAnglesCache.valid = true;
                _worldEulerAnglesCache.value = value;
            }
        }

        public FVector3 localEulerAngles
        {
            get
            {
                if(_localEulerAnglesCache.valid)
                {
                    return _localEulerAnglesCache.value;
                }
                else
                {
                    _localEulerAnglesCache.valid = true;

                    _localEulerAnglesCache.value = _localRotation.eulerAngles;

                    return _localEulerAnglesCache.value;
                }
            }
            set
            {
                if(_localEulerAnglesCache.valid)
                {
                    if(_localEulerAnglesCache.value == value)
                    {
                        return;
                    }
                }

                localRotation = FQuaternion.FromEulerRad1(value * FMath.Deg2Rad);

                _localEulerAnglesCache.valid = true;
                _localEulerAnglesCache.value = value;
            }
        }

        public FQuaternion rotation
        {
            get{
                if(_worldQuaternionCache.valid)
                {
                    return _worldQuaternionCache.value;
                }

                _worldQuaternionCache.valid = true;

                if(parent == null)
                {
                    _worldQuaternionCache.value = _localRotation;
                }
                else
                {
                    Quaternion rot = (Quaternion)parent.rotation;
                    Quaternion rot1 = (Quaternion)_localRotation;
                    Quaternion result = rot * rot1;

                    _worldQuaternionCache.value = parent.rotation * _localRotation;

                    //bool equal = _worldQuaternionCache.value.Equals(result);
                    //bool equal1 = _worldQuaternionCache.value.Equals(transform.rotation);

                    //Debug.Log($"calculation {equal}");
                    //Debug.Log($"compare Transform parent={transform.parent.rotation.ToString("F4")} local={transform.localRotation.ToString("F4")} mul={transform.parent.rotation* transform.localRotation}");
                    //Debug.Log($"compare Ftransform parent={parent.rotation} local={_localRotation} mul={parent.rotation*_localRotation}");
                    //Debug.Log($"compare {equal1} {transform.rotation} : {_worldQuaternionCache.value}");
                }

                return _worldQuaternionCache.value;
            }
            set{
                if(_worldQuaternionCache.valid)
                {
                    if(_worldQuaternionCache.value == value)
                    {
                        return;
                    }
                }

                if(parent == null)
                {
                    localRotation = value;
                }
                else
                {
                    FQuaternion newLocalRotation = parent.inverseRotation * value;
                    localRotation = newLocalRotation;
                }

                _worldQuaternionCache.valid = true;
                _worldQuaternionCache.value = value;
            }
        }

        FQuaternion inverseRotation
        {
            get
            {
                FQuaternion inverseRotation = FQuaternion.Inverse(rotation);
                return inverseRotation;
            }
        }

        public FQuaternion localRotation
        {
            get{
                return _localRotation;
            }
            set{
                if(_localRotation == value)
                {
                    return;
                }

                _localRotation = value;
                _localEulerAnglesCache.valid = false;
                _worldQuaternionCache.valid = false;
                _worldEulerAnglesCache.valid = false;
                _localToWorldMatrixCache.valid = false;
                _worldToLocalMatrixCache.valid = false;

                for(int i = 0; i < _children.Count; i++)
                {
                    FTransform child = _children[i];
                    child._worldPositionCache.valid = false;
                    child._worldQuaternionCache.valid = false;
                    child._worldEulerAnglesCache.valid = false;
                    child._localToWorldMatrixCache.valid = false;
                    child._worldToLocalMatrixCache.valid = false;
                }
            }
        }

        public FVector3 localPosition
        {
            get{
                return _localPosition;
            }
            set{
                if(_localPosition == value)
                {
                    return;
                }

                _localPosition = value;
                _worldPositionCache.valid = false;
                _localToWorldMatrixCache.valid = false;
                _worldToLocalMatrixCache.valid = false;

                for (int i = 0; i < _children.Count; i++)
                {
                    FTransform child = _children[i];
                    child._worldPositionCache.valid = false;
                    child._localToWorldMatrixCache.valid = false;
                    child._worldToLocalMatrixCache.valid = false;
                }
            }
        }

        public FVector3 position
        {
            get{
                if(_worldPositionCache.valid)
                {
                    return _worldPositionCache.value;
                }

                _worldPositionCache.valid = true;

                if(parent == null)
                {
                    _worldPositionCache.value = _localPosition;
                }
                else
                {
                    FMatrix4x4 m = parent.localToWorldMatrix;
                    _worldPositionCache.value = m.MultiplyPoint3x4(_localPosition);
                }

                return _worldPositionCache.value;
            }
            set{
                if(_worldPositionCache.valid)
                {
                    if(_worldPositionCache.value == value)
                    {
                        return;
                    }
                }

                if(parent == null)
                {
                    localPosition = value;
                }
                else
                {
                    FMatrix4x4 m = parent.worldToLocalMatrix;
                    FVector3 p = m.MultiplyPoint3x4(value);

                    localPosition = p;
                }

                _worldPositionCache.valid = true;
                _worldPositionCache.value = value;
            }
        }

        public FVector3 localScale
        {
            get{
                return _localScale;
            }
            set{
                if(_localScale == value)
                {
                    return;
                }
                _localScale = value;
                _localToWorldMatrixCache.valid = false;
                _worldToLocalMatrixCache.valid = false;

                for (int i = 0; i < _children.Count; i++)
                {
                    FTransform child = _children[i];
                    child._worldPositionCache.valid = false;
                    child._localToWorldMatrixCache.valid = false;
                    child._worldToLocalMatrixCache.valid = false;
                }
            }
        }

        public FVector3 right
        {
            get
            {
                return rotation * FVector3.right;
            }
        }

        public FVector3 up
        {
            get
            {
                return rotation * FVector3.up;
            }
        }

        public FVector3 forward
        {
            get
            {
                return rotation * FVector3.forward;
            }
        }

        public FMatrix4x4 localToWorldMatrix
        {
            get
            {
                if(_localToWorldMatrixCache.valid)
                {
                    return _localToWorldMatrixCache.value;
                }

                _localToWorldMatrixCache.valid = true;

                if (parent == null)
                {
                    _localToWorldMatrixCache.value = FMatrix4x4.TRS(_localPosition, _localRotation, _localScale);
                }
                else
                {
                    _localToWorldMatrixCache.value = parent.localToWorldMatrix * FMatrix4x4.TRS(_localPosition, _localRotation, _localScale);
                }

                return _localToWorldMatrixCache.value;
            }
        }

        public FMatrix4x4 localToWorldMatrixUnscaled
        {
            get
            {
                if (parent == null)
                {
                    return FMatrix4x4.TRS(_localPosition, _localRotation, FVector3.one);
                }
                else
                {
                    return parent.localToWorldMatrixUnscaled * FMatrix4x4.TRS(_localPosition, _localRotation, FVector3.one);
                }
            }
        }

        public FMatrix4x4 WorldToLocalMatrixUnscaled
        {
            get
            {
                return FMatrix4x4.Inverse(localToWorldMatrixUnscaled);
            }
        }

        public FMatrix4x4 worldToLocalMatrix
        {
            get
            {
                if (_worldToLocalMatrixCache.valid)
                {
                    return _worldToLocalMatrixCache.value;
                }

                _worldToLocalMatrixCache.valid = true;

                _worldToLocalMatrixCache.value = FMatrix4x4.Inverse(localToWorldMatrix);

                return _worldToLocalMatrixCache.value;
            }
        }

        //============================== Public Methods ==============================
        //https://answers.unity.com/questions/1021968/difference-between-transformtransformvector-and-tr.html

        //rotation only
        /// <summary>
        /// Transforms a direction from world space to local space. The opposite of Transform.TransformDirection.
        /// This operation is unaffected by scale.
        /// You should use Transform.InverseTransformPoint if the vector represents a position in space rather than a direction.
        /// </summary>
        /// <param name="direction"></param>
        /// <returns></returns>
        public FVector3 InverseTransformDirection(FVector3 direction)
        {
            FMatrix4x4 m = WorldToLocalMatrixUnscaled;
            FVector3 p = m.MultiplyVector(direction);
            return p;
        }

        //position, rotation, and scale
        public FVector3 InverseTransformPoint(FVector3 point)
        {
            FMatrix4x4 m = worldToLocalMatrix;
            FVector3 p = m.MultiplyPoint3x4(point);
            return p;
        }

        //rotation and scale only
        public FVector3 InverseTransformVector(FVector3 vector)
        {
            FMatrix4x4 m = worldToLocalMatrix;
            FVector3 p = m.MultiplyVector(vector);
            return p;
        }

        public FVector3 TransformDirection(FVector3 direction)
        {
            FMatrix4x4 m = localToWorldMatrixUnscaled;
            FVector3 p = m.MultiplyVector(direction);
            return p;
        }

        //position, rotation, and scale
        public FVector3 TransformPoint(FVector3 point)
        {
            FMatrix4x4 m = localToWorldMatrix;
            FVector3 p = m.MultiplyPoint3x4(point);
            return p;
        }

        //rotation and scale only
        public FVector3 TransformVector(FVector3 vector)
        {
            FMatrix4x4 m = localToWorldMatrix;
            FVector3 p = m.MultiplyVector(vector);
            return p;
        }

        public void LookAt(FTransform target)
        {
            LookAt(target, FVector3.up);
        }

        public void LookAt(FTransform target, FVector3 worldUp)
        {
            LookAt(target.position, worldUp);
        }

        public void LookAt(FVector3 worldPosition, FVector3 worldUp)
        {
            FVector3 dir = (worldPosition - position).normalized;
            rotation = FQuaternion.LookRotation(dir, worldUp);
        }

        public void LookAt(FVector3 worldPosition)
        {
            FVector3 up = FVector3.up;
            FVector3 dir = (worldPosition - position).normalized;
            rotation = FQuaternion.LookRotation(dir, up);
        }

        //
        // Summary:
        //     Applies a rotation of eulerAngles.z degrees around the z-axis, eulerAngles.x
        //     degrees around the x-axis, and eulerAngles.y degrees around the y-axis (in that
        //     order).
        //
        // Parameters:
        //   eulers:
        //     The rotation to apply in euler angles.
        //
        //   relativeTo:
        //     Determines whether to rotate the GameObject either locally to the GameObject
        //     or relative to the Scene in world space.
        public void Rotate(FVector3 eulers, Space relativeTo = Space.Self)
        {
            FQuaternion quat = FQuaternion.FromEulerRad1(eulers * FMath.Deg2Rad);

            if (relativeTo == Space.Self)
            {
                localRotation *= quat;
            }
            else
            {
                rotation *= FQuaternion.Inverse(rotation) * quat * rotation;
            }
        }
        //
        // Summary:
        //     Applies a rotation of eulerAngles.z degrees around the z-axis, eulerAngles.x
        //     degrees around the x-axis, and eulerAngles.y degrees around the y-axis (in that
        //     order).
        //
        // Parameters:
        //   eulers:
        //     The rotation to apply in euler angles.
        public void Rotate(FVector3 eulers)
        {
            Rotate(eulers, Space.Self);
        }
        //
        // Summary:
        //     The implementation of this method applies a rotation of zAngle degrees around
        //     the z axis, xAngle degrees around the x axis, and yAngle degrees around the y
        //     axis (in that order).
        //
        // Parameters:
        //   relativeTo:
        //     Determines whether to rotate the GameObject either locally to the GameObject
        //     or relative to the Scene in world space.
        //
        //   xAngle:
        //     Degrees to rotate the GameObject around the X axis.
        //
        //   yAngle:
        //     Degrees to rotate the GameObject around the Y axis.
        //
        //   zAngle:
        //     Degrees to rotate the GameObject around the Z axis.
        public void Rotate(FFloat xAngle, FFloat yAngle, FFloat zAngle, Space relativeTo = Space.Self)
        {
            Rotate(new FVector3(xAngle, yAngle, zAngle), relativeTo);
        }
        //
        // Summary:
        //     Rotates the object around the given axis by the number of degrees defined by
        //     the given angle.
        //
        // Parameters:
        //   angle:
        //     The degrees of rotation to apply.
        //
        //   axis:
        //     The axis to apply rotation to.
        //
        //   relativeTo:
        //     Determines whether to rotate the GameObject either locally to the GameObject
        //     or relative to the Scene in world space.
        public void Rotate(FVector3 axis, FFloat angle, Space relativeTo = Space.Self)
        {
            if (relativeTo == Space.Self)
            {
                FVector3 selfAxis = TransformDirection(axis);
                FQuaternion q = FQuaternion.AngleAxis(angle, selfAxis);
                Quaternion uq = Quaternion.AngleAxis((float)angle, (Vector3)selfAxis);

                if (!q.Equals(uq))
                {
                    Debug.LogError($"Not equal uq={uq} fq={q}");
                }

                rotation = q * rotation;
            }
            else
            {
                FQuaternion q = FQuaternion.AngleAxis(angle, axis);
                Quaternion uq = Quaternion.AngleAxis((float)angle, (Vector3)axis);

                //Debug.Log($"uq={uq.ToString("F4")}");
                //Debug.Log($"fq={q}");

                if (!q.Equals(uq))
                {
                    Debug.LogError($"Not equal uq={uq} fq={q}");
                }

                rotation = q * rotation;
            }
        }
        //
        // Summary:
        //     Rotates the object around the given axis by the number of degrees defined by
        //     the given angle.
        //
        // Parameters:
        //   axis:
        //     The axis to apply rotation to.
        //
        //   angle:
        //     The degrees of rotation to apply.
        public void Rotate(FVector3 axis, FFloat angle)
        {
            Rotate(axis, angle, Space.Self);
        }

        //
        // Summary:
        //     Rotates the transform about axis passing through point in world coordinates by
        //     angle degrees.
        //
        // Parameters:
        //   point:
        //
        //   axis:
        //
        //   angle:
        public void RotateAround(FVector3 point, FVector3 axis, FFloat angle)
        {
            FVector3 worldPos = position;
            FQuaternion q = FQuaternion.AngleAxis(angle, axis);
            FVector3 dif = worldPos - point;
            dif = q * dif;
            worldPos = point + dif;
            position = worldPos;
            rotation *= q;
        }

        //
        // Summary:
        //     Sets the world space position and rotation of the Transform component.
        //
        // Parameters:
        //   position:
        //
        //   rotation:
        public void SetPositionAndRotation(FVector3 position, FQuaternion rotation)
        {
            this.position = position;
            this.rotation = rotation;
        }

        //
        // Summary:
        //     Moves the transform by x along the x axis, y along the y axis, and z along the
        //     z axis.
        //
        // Parameters:
        //   x:
        //
        //   y:
        //
        //   z:
        //
        //   relativeTo:
        public void Translate(FFloat x, FFloat y, FFloat z)
        {
            Translate(new FVector3(x, y, z));
        }
        //
        // Summary:
        //     Moves the transform by x along the x axis, y along the y axis, and z along the
        //     z axis.
        //
        // Parameters:
        //   x:
        //
        //   y:
        //
        //   z:
        //
        //   relativeTo:
        public void Translate(FFloat x, FFloat y, FFloat z, Space relativeTo = Space.Self)
        {
            Translate(new FVector3(x, y, z), relativeTo);
        }
        //
        // Summary:
        //     Moves the transform in the direction and distance of translation.
        //
        // Parameters:
        //   translation:
        //
        //   relativeTo:
        public void Translate(FVector3 translation)
        {
            Translate(translation, Space.Self);
        }
        //
        // Summary:
        //     Moves the transform in the direction and distance of translation.
        //
        // Parameters:
        //   translation:
        //
        //   relativeTo:
        public void Translate(FVector3 translation, Space relativeTo = Space.Self)
        {
            if (relativeTo == Space.World)
            {
                this.position += translation;
            }
            else
            {
                this.position += this.TransformDirection(translation);
            }
        }
        //
        // Summary:
        //     Moves the transform by x along the x axis, y along the y axis, and z along the
        //     z axis.
        //
        // Parameters:
        //   x:
        //
        //   y:
        //
        //   z:
        //
        //   relativeTo:
        public void Translate(FFloat x, FFloat y, FFloat z, FTransform relativeTo)
        {
            Translate(new FVector3(x, y, z), relativeTo);
        }
        //
        // Summary:
        //     Moves the transform in the direction and distance of translation.
        //
        // Parameters:
        //   translation:
        //
        //   relativeTo:
        public void Translate(FVector3 translation, FTransform relativeTo)
        {
            if (relativeTo)
            {
                this.position += relativeTo.TransformDirection(translation);
            }
            else
            {
                this.position += translation;
            }
        }
        //============================== Unity Events ==============================

        public void UpdateHierarchy(FTransform newParent)
        {
            if (_parent != null)
            {
                _parent._children.Remove(this);
                UnityEditor.EditorUtility.SetDirty(_parent);
            }

            if (newParent != null)
            {
                if (!newParent._children.Contains(this))
                {
                    newParent._children.Add(this);
                }

                UnityEditor.EditorUtility.SetDirty(newParent);
            }

            _parent = newParent; 
        }

        void Update()
        {
#if UNITY_EDITOR
            if (!Application.isPlaying)
            {
                ImportFromUnity();
            }
#endif
            if(Application.isPlaying)
            {
                //if (transform.hasChanged)
                //{
                //    transform.hasChanged = false;
                //    ImportTransformFromUnity();
                //}
                if(!interpolation)
                {
                    ExportTransformToUnity();
                    //transform.hasChanged = false;
                }
                //
            }
        }

        void OnDestroy()
        {
#if UNITY_EDITOR
            if (!Application.isPlaying)
            {
                if (parent != null)
                {
                    parent._children.Remove(this);
                    UnityEditor.EditorUtility.SetDirty(parent);
                }
            }
#endif
        }

        private void Reset()
        {
#if UNITY_EDITOR
            if (!Application.isPlaying)
            {
                ImportFromUnity(true);
            }
#endif
        }

        void ImportFromUnity(bool reset = false)
        {
            if (transform.parent != null)
            {
                FTransform fParent = transform.parent.GetComponent<FTransform>();

                if (fParent == null)
                {
                    throw new Exception($"{name} parent missing FTransform component");
                }

                if (_parent == null || fParent != _parent)
                {
                    UpdateHierarchy(fParent);
                    UnityEditor.EditorUtility.SetDirty(this);
                }
            }

            if(reset || transform.hasChanged)
            {
                transform.hasChanged = false;
                bool changed = ImportTransformFromUnity();
                if(changed)
                {
                    UnityEditor.EditorUtility.SetDirty(this);
                }
            }
        }

        void ExportTransformToUnity()
        {
            transform.localPosition = (Vector3)localPosition;
            transform.localRotation = (Quaternion)localRotation;
            transform.localScale = (Vector3)localScale;
        }

        bool ImportTransformFromUnity()
        {
            bool equal = localPosition.Equals(transform.localPosition);
            equal &= localRotation.Equals(transform.localRotation);
            equal &= localScale.Equals(transform.localScale);

            if(!equal)
            {
                localPosition = (FVector3)transform.localPosition;
                localRotation = (FQuaternion)transform.localRotation;
                localScale = (FVector3)transform.localScale;
            }

            return !equal;
        }
    }
}